import cv2
from cv_bridge import CvBridge


class CameraProcessor:
    def __init__(self):
        """初始化相机处理器."""
        self.bridge = CvBridge()

    def process_image(self, img_msg):
        """处理图像消息."""
        try:
            cv_image = self.bridge.imgmsg_to_cv2(img_msg, "bgr8")
            return cv_image
        except Exception as e:
            print(f"Error converting image: {e!s}")
            return None

    def draw_detections(self, image, detections, distances=None):
        """在图像上绘制检测结果."""
        for i, det in enumerate(detections):
            x1, y1, x2, y2 = map(int, det["bbox"])
            conf = det["confidence"]

            # 绘制边界框
            cv2.rectangle(image, (x1, y1), (x2, y2), (0, 255, 0), 2)

            # 添加置信度标签
            label = f"Conf: {conf:.2f}"
            if distances and i < len(distances):
                label += f" Dist: {distances[i]:.2f}m"

            cv2.putText(
                image,
                label,
                (x1, y1 - 10),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.5,
                (0, 255, 0),
                2,
            )

        return image
